//
// Academic License - for use in teaching, academic research, and meeting
// course requirements at degree granting institutions only.  Not for
// government, commercial, or other organizational use.
//
// File: sim_kinematics.cpp
//
// Code generated for Simulink model 'sim_kinematics'.
//
// Model version                  : 1.47
// Simulink Coder version         : 9.8 (R2022b) 13-May-2022
// C/C++ source code generated on : Wed May 24 19:18:07 2023
//
// Target selection: ert.tlc
// Embedded hardware selection: ARM Compatible->ARM 64-bit (LP64)
// Code generation objectives: Unspecified
// Validation result: Not run
//
#include "sim_kinematics.h"
#include <math.h>
#include "rtwtypes.h"
#include "sim_kinematics_types.h"

// Block signals (default storage)
B_sim_kinematics_T sim_kinematics_B;

// Block states (default storage)
DW_sim_kinematics_T sim_kinematics_DW;

// Real-time model
RT_MODEL_sim_kinematics_T sim_kinematics_M_ = RT_MODEL_sim_kinematics_T();
RT_MODEL_sim_kinematics_T *const sim_kinematics_M = &sim_kinematics_M_;

// Model step function
void sim_kinematics_step(void)
{
  SL_Bus_sim_kinematics_std_msgs_Float64 rtb_BusAssignment1_f;
  SL_Bus_sim_kinematics_std_msgs_Float64 rtb_BusAssignment2;
  SL_Bus_sim_kinematics_std_msgs_Float64 rtb_BusAssignment2_e;
  SL_Bus_sim_kinematics_std_msgs_Float64 rtb_BusAssignment3;
  SL_Bus_sim_kinematics_std_msgs_Float64 rtb_BusAssignment3_j;
  SL_Bus_sim_kinematics_std_msgs_Float64 rtb_BusAssignment_l;
  boolean_T b_varargout_1;

  // Outputs for Atomic SubSystem: '<Root>/usr_message'
  // MATLABSystem: '<S2>/SourceBlock'
  b_varargout_1 = Sub_sim_kinematics_42.getLatestMessage
    (&sim_kinematics_B.b_varargout_2);

  // Outputs for Enabled SubSystem: '<Root>/motion controller' incorporates:
  //   EnablePort: '<S1>/Enable'

  // Outputs for Enabled SubSystem: '<S2>/Enabled Subsystem' incorporates:
  //   EnablePort: '<S18>/Enable'

  if (b_varargout_1) {
    real_T rtb_BusAssignment_tmp;

    // MATLAB Function: '<S6>/Front Steering' incorporates:
    //   SignalConversion generated from: '<S7>/ SFunction '

    if (!sim_kinematics_DW.robot_not_empty) {
      sim_kinematics_DW.robot.tolerance = 1.0E-12;
      sim_kinematics_DW.robot.wheelRadius =
        sim_kinematics_P.FourWheelSteeringInverseKinem_i;
      sim_kinematics_DW.robot.frontWheelDist =
        sim_kinematics_P.FourWheelSteeringInverseKinemat;
      sim_kinematics_DW.robot.rearWheelDist =
        sim_kinematics_P.FourWheelSteeringInverseKinem_a;
      sim_kinematics_DW.robot_not_empty = true;
    }

    if (fabs(sim_kinematics_B.b_varargout_2.Linear.X) >
        sim_kinematics_DW.robot.tolerance) {
      sim_kinematics_B.phiFront = atan((sim_kinematics_DW.robot.frontWheelDist +
        sim_kinematics_DW.robot.rearWheelDist) *
        sim_kinematics_B.b_varargout_2.Angular.Z /
        sim_kinematics_B.b_varargout_2.Linear.X);
    } else {
      sim_kinematics_B.phiFront = 0.0;
    }

    rtb_BusAssignment_tmp = sim_kinematics_B.b_varargout_2.Linear.X / cos
      (sim_kinematics_B.phiFront) / sim_kinematics_DW.robot.wheelRadius;

    // BusAssignment: '<S4>/Bus Assignment' incorporates:
    //   MATLAB Function: '<S6>/Front Steering'

    sim_kinematics_B.BusAssignment.Data = rtb_BusAssignment_tmp;

    // Outputs for Atomic SubSystem: '<S4>/Publish'
    // MATLABSystem: '<S9>/SinkBlock'
    Pub_sim_kinematics_11.publish(&sim_kinematics_B.BusAssignment);

    // End of Outputs for SubSystem: '<S4>/Publish'

    // BusAssignment: '<S4>/Bus Assignment1'
    sim_kinematics_B.BusAssignment1.Data = rtb_BusAssignment_tmp;

    // Outputs for Atomic SubSystem: '<S4>/Publish1'
    // MATLABSystem: '<S10>/SinkBlock'
    Pub_sim_kinematics_20.publish(&sim_kinematics_B.BusAssignment1);

    // End of Outputs for SubSystem: '<S4>/Publish1'

    // MATLAB Function: '<S6>/Front Steering' incorporates:
    //   SignalConversion generated from: '<S7>/ SFunction '

    rtb_BusAssignment_tmp = sim_kinematics_B.b_varargout_2.Linear.X /
      sim_kinematics_DW.robot.wheelRadius;

    // BusAssignment: '<S4>/Bus Assignment2' incorporates:
    //   MATLAB Function: '<S6>/Front Steering'

    rtb_BusAssignment2.Data = rtb_BusAssignment_tmp;

    // Outputs for Atomic SubSystem: '<S4>/Publish2'
    // MATLABSystem: '<S11>/SinkBlock'
    Pub_sim_kinematics_21.publish(&rtb_BusAssignment2);

    // End of Outputs for SubSystem: '<S4>/Publish2'

    // BusAssignment: '<S4>/Bus Assignment3'
    rtb_BusAssignment3.Data = rtb_BusAssignment_tmp;

    // Outputs for Atomic SubSystem: '<S4>/Publish3'
    // MATLABSystem: '<S12>/SinkBlock'
    Pub_sim_kinematics_23.publish(&rtb_BusAssignment3);

    // End of Outputs for SubSystem: '<S4>/Publish3'

    // BusAssignment: '<S5>/Bus Assignment' incorporates:
    //   MATLAB Function: '<S6>/Front Steering'

    rtb_BusAssignment_l.Data = sim_kinematics_B.phiFront;

    // Outputs for Atomic SubSystem: '<S5>/Publish'
    // MATLABSystem: '<S14>/SinkBlock'
    Pub_sim_kinematics_37.publish(&rtb_BusAssignment_l);

    // End of Outputs for SubSystem: '<S5>/Publish'

    // BusAssignment: '<S5>/Bus Assignment1' incorporates:
    //   MATLAB Function: '<S6>/Front Steering'

    rtb_BusAssignment1_f.Data = sim_kinematics_B.phiFront;

    // Outputs for Atomic SubSystem: '<S5>/Publish1'
    // MATLABSystem: '<S15>/SinkBlock'
    Pub_sim_kinematics_38.publish(&rtb_BusAssignment1_f);

    // End of Outputs for SubSystem: '<S5>/Publish1'

    // BusAssignment: '<S5>/Bus Assignment2'
    rtb_BusAssignment2_e.Data = 0.0;

    // Outputs for Atomic SubSystem: '<S5>/Publish2'
    // MATLABSystem: '<S16>/SinkBlock'
    Pub_sim_kinematics_39.publish(&rtb_BusAssignment2_e);

    // End of Outputs for SubSystem: '<S5>/Publish2'

    // BusAssignment: '<S5>/Bus Assignment3'
    rtb_BusAssignment3_j.Data = 0.0;

    // Outputs for Atomic SubSystem: '<S5>/Publish3'
    // MATLABSystem: '<S17>/SinkBlock'
    Pub_sim_kinematics_40.publish(&rtb_BusAssignment3_j);

    // End of Outputs for SubSystem: '<S5>/Publish3'
  }

  // End of MATLABSystem: '<S2>/SourceBlock'
  // End of Outputs for SubSystem: '<S2>/Enabled Subsystem'
  // End of Outputs for SubSystem: '<Root>/motion controller'
  // End of Outputs for SubSystem: '<Root>/usr_message'
}

// Model initialize function
void sim_kinematics_initialize(void)
{
  {
    char_T b_zeroDelimTopic_0[46];
    char_T b_zeroDelimTopic[11];
    static const char_T tmp[10] = { 'z', 'e', 'u', 's', '_', 's', 'p', 'e', 'e',
      'd' };

    static const char_T tmp_0[46] = { '/', 'z', 'e', 'u', 's', '_', 's', '3',
      '/', 'd', 'y', 'n', 'a', 'm', 'i', 'c', '_', 'f', 'r', 'o', 'n', 't', '_',
      'l', 'e', 'f', 't', '_', 'c', 'o', 'n', 't', 'r', 'o', 'l', 'l', 'e', 'r',
      '/', 'c', 'o', 'm', 'm', 'a', 'n', 'd' };

    static const char_T tmp_1[47] = { '/', 'z', 'e', 'u', 's', '_', 's', '3',
      '/', 'd', 'y', 'n', 'a', 'm', 'i', 'c', '_', 'f', 'r', 'o', 'n', 't', '_',
      'r', 'i', 'g', 'h', 't', '_', 'c', 'o', 'n', 't', 'r', 'o', 'l', 'l', 'e',
      'r', '/', 'c', 'o', 'm', 'm', 'a', 'n', 'd' };

    static const char_T tmp_2[45] = { '/', 'z', 'e', 'u', 's', '_', 's', '3',
      '/', 'd', 'y', 'n', 'a', 'm', 'i', 'c', '_', 'r', 'e', 'a', 'r', '_', 'l',
      'e', 'f', 't', '_', 'c', 'o', 'n', 't', 'r', 'o', 'l', 'l', 'e', 'r', '/',
      'c', 'o', 'm', 'm', 'a', 'n', 'd' };

    static const char_T tmp_3[46] = { '/', 'z', 'e', 'u', 's', '_', 's', '3',
      '/', 'd', 'y', 'n', 'a', 'm', 'i', 'c', '_', 'r', 'e', 'a', 'r', '_', 'r',
      'i', 'g', 'h', 't', '_', 'c', 'o', 'n', 't', 'r', 'o', 'l', 'l', 'e', 'r',
      '/', 'c', 'o', 'm', 'm', 'a', 'n', 'd' };

    static const char_T tmp_4[47] = { '/', 'z', 'e', 'u', 's', '_', 's', '3',
      '/', 's', 't', 'e', 'e', 'r', 'i', 'n', 'g', '_', 'f', 'r', 'o', 'n', 't',
      '_', 'l', 'e', 'f', 't', '_', 'c', 'o', 'n', 't', 'r', 'o', 'l', 'l', 'e',
      'r', '/', 'c', 'o', 'm', 'm', 'a', 'n', 'd' };

    static const char_T tmp_5[48] = { '/', 'z', 'e', 'u', 's', '_', 's', '3',
      '/', 's', 't', 'e', 'e', 'r', 'i', 'n', 'g', '_', 'f', 'r', 'o', 'n', 't',
      '_', 'r', 'i', 'g', 'h', 't', '_', 'c', 'o', 'n', 't', 'r', 'o', 'l', 'l',
      'e', 'r', '/', 'c', 'o', 'm', 'm', 'a', 'n', 'd' };

    static const char_T tmp_6[46] = { '/', 'z', 'e', 'u', 's', '_', 's', '3',
      '/', 's', 't', 'e', 'e', 'r', 'i', 'n', 'g', '_', 'r', 'e', 'a', 'r', '_',
      'l', 'e', 'f', 't', '_', 'c', 'o', 'n', 't', 'r', 'o', 'l', 'l', 'e', 'r',
      '/', 'c', 'o', 'm', 'm', 'a', 'n', 'd' };

    static const char_T tmp_7[47] = { '/', 'z', 'e', 'u', 's', '_', 's', '3',
      '/', 's', 't', 'e', 'e', 'r', 'i', 'n', 'g', '_', 'r', 'e', 'a', 'r', '_',
      'r', 'i', 'g', 'h', 't', '_', 'c', 'o', 'n', 't', 'r', 'o', 'l', 'l', 'e',
      'r', '/', 'c', 'o', 'm', 'm', 'a', 'n', 'd' };

    // SystemInitialize for Atomic SubSystem: '<Root>/usr_message'
    // Start for MATLABSystem: '<S2>/SourceBlock'
    sim_kinematics_DW.obj_d.matlabCodegenIsDeleted = false;
    sim_kinematics_DW.obj_d.isInitialized = 1;
    for (int32_T i = 0; i < 10; i++) {
      b_zeroDelimTopic[i] = tmp[i];
    }

    b_zeroDelimTopic[10] = '\x00';
    Sub_sim_kinematics_42.createSubscriber(&b_zeroDelimTopic[0], 1);
    sim_kinematics_DW.obj_d.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S2>/SourceBlock'
    // End of SystemInitialize for SubSystem: '<Root>/usr_message'

    // SystemInitialize for Enabled SubSystem: '<Root>/motion controller'
    // SystemInitialize for Atomic SubSystem: '<S4>/Publish'
    // Start for MATLABSystem: '<S9>/SinkBlock'
    sim_kinematics_DW.obj_c.matlabCodegenIsDeleted = false;
    sim_kinematics_DW.obj_c.isInitialized = 1;
    for (int32_T i = 0; i < 46; i++) {
      sim_kinematics_B.b_zeroDelimTopic_c[i] = tmp_0[i];
    }

    sim_kinematics_B.b_zeroDelimTopic_c[46] = '\x00';
    Pub_sim_kinematics_11.createPublisher(&sim_kinematics_B.b_zeroDelimTopic_c[0],
      1);
    sim_kinematics_DW.obj_c.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S9>/SinkBlock'
    // End of SystemInitialize for SubSystem: '<S4>/Publish'

    // SystemInitialize for Atomic SubSystem: '<S4>/Publish1'
    // Start for MATLABSystem: '<S10>/SinkBlock'
    sim_kinematics_DW.obj_f.matlabCodegenIsDeleted = false;
    sim_kinematics_DW.obj_f.isInitialized = 1;
    for (int32_T i = 0; i < 47; i++) {
      sim_kinematics_B.b_zeroDelimTopic_m[i] = tmp_1[i];
    }

    sim_kinematics_B.b_zeroDelimTopic_m[47] = '\x00';
    Pub_sim_kinematics_20.createPublisher(&sim_kinematics_B.b_zeroDelimTopic_m[0],
      1);
    sim_kinematics_DW.obj_f.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S10>/SinkBlock'
    // End of SystemInitialize for SubSystem: '<S4>/Publish1'

    // SystemInitialize for Atomic SubSystem: '<S4>/Publish2'
    // Start for MATLABSystem: '<S11>/SinkBlock'
    sim_kinematics_DW.obj_m.matlabCodegenIsDeleted = false;
    sim_kinematics_DW.obj_m.isInitialized = 1;
    for (int32_T i = 0; i < 45; i++) {
      b_zeroDelimTopic_0[i] = tmp_2[i];
    }

    b_zeroDelimTopic_0[45] = '\x00';
    Pub_sim_kinematics_21.createPublisher(&b_zeroDelimTopic_0[0], 1);
    sim_kinematics_DW.obj_m.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S11>/SinkBlock'
    // End of SystemInitialize for SubSystem: '<S4>/Publish2'

    // SystemInitialize for Atomic SubSystem: '<S4>/Publish3'
    // Start for MATLABSystem: '<S12>/SinkBlock'
    sim_kinematics_DW.obj_l.matlabCodegenIsDeleted = false;
    sim_kinematics_DW.obj_l.isInitialized = 1;
    for (int32_T i = 0; i < 46; i++) {
      sim_kinematics_B.b_zeroDelimTopic_c[i] = tmp_3[i];
    }

    sim_kinematics_B.b_zeroDelimTopic_c[46] = '\x00';
    Pub_sim_kinematics_23.createPublisher(&sim_kinematics_B.b_zeroDelimTopic_c[0],
      1);
    sim_kinematics_DW.obj_l.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S12>/SinkBlock'
    // End of SystemInitialize for SubSystem: '<S4>/Publish3'

    // SystemInitialize for Atomic SubSystem: '<S5>/Publish'
    // Start for MATLABSystem: '<S14>/SinkBlock'
    sim_kinematics_DW.obj_o.matlabCodegenIsDeleted = false;
    sim_kinematics_DW.obj_o.isInitialized = 1;
    for (int32_T i = 0; i < 47; i++) {
      sim_kinematics_B.b_zeroDelimTopic_m[i] = tmp_4[i];
    }

    sim_kinematics_B.b_zeroDelimTopic_m[47] = '\x00';
    Pub_sim_kinematics_37.createPublisher(&sim_kinematics_B.b_zeroDelimTopic_m[0],
      1);
    sim_kinematics_DW.obj_o.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S14>/SinkBlock'
    // End of SystemInitialize for SubSystem: '<S5>/Publish'

    // SystemInitialize for Atomic SubSystem: '<S5>/Publish1'
    // Start for MATLABSystem: '<S15>/SinkBlock'
    sim_kinematics_DW.obj_g.matlabCodegenIsDeleted = false;
    sim_kinematics_DW.obj_g.isInitialized = 1;
    for (int32_T i = 0; i < 48; i++) {
      sim_kinematics_B.b_zeroDelimTopic[i] = tmp_5[i];
    }

    sim_kinematics_B.b_zeroDelimTopic[48] = '\x00';
    Pub_sim_kinematics_38.createPublisher(&sim_kinematics_B.b_zeroDelimTopic[0],
      1);
    sim_kinematics_DW.obj_g.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S15>/SinkBlock'
    // End of SystemInitialize for SubSystem: '<S5>/Publish1'

    // SystemInitialize for Atomic SubSystem: '<S5>/Publish2'
    // Start for MATLABSystem: '<S16>/SinkBlock'
    sim_kinematics_DW.obj_p.matlabCodegenIsDeleted = false;
    sim_kinematics_DW.obj_p.isInitialized = 1;
    for (int32_T i = 0; i < 46; i++) {
      sim_kinematics_B.b_zeroDelimTopic_c[i] = tmp_6[i];
    }

    sim_kinematics_B.b_zeroDelimTopic_c[46] = '\x00';
    Pub_sim_kinematics_39.createPublisher(&sim_kinematics_B.b_zeroDelimTopic_c[0],
      1);
    sim_kinematics_DW.obj_p.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S16>/SinkBlock'
    // End of SystemInitialize for SubSystem: '<S5>/Publish2'

    // SystemInitialize for Atomic SubSystem: '<S5>/Publish3'
    // Start for MATLABSystem: '<S17>/SinkBlock'
    sim_kinematics_DW.obj.matlabCodegenIsDeleted = false;
    sim_kinematics_DW.obj.isInitialized = 1;
    for (int32_T i = 0; i < 47; i++) {
      sim_kinematics_B.b_zeroDelimTopic_m[i] = tmp_7[i];
    }

    sim_kinematics_B.b_zeroDelimTopic_m[47] = '\x00';
    Pub_sim_kinematics_40.createPublisher(&sim_kinematics_B.b_zeroDelimTopic_m[0],
      1);
    sim_kinematics_DW.obj.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S17>/SinkBlock'
    // End of SystemInitialize for SubSystem: '<S5>/Publish3'
    // End of SystemInitialize for SubSystem: '<Root>/motion controller'
  }
}

// Model terminate function
void sim_kinematics_terminate(void)
{
  // Terminate for Atomic SubSystem: '<Root>/usr_message'
  // Terminate for MATLABSystem: '<S2>/SourceBlock'
  if (!sim_kinematics_DW.obj_d.matlabCodegenIsDeleted) {
    sim_kinematics_DW.obj_d.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S2>/SourceBlock'
  // End of Terminate for SubSystem: '<Root>/usr_message'

  // Terminate for Enabled SubSystem: '<Root>/motion controller'
  // Terminate for Atomic SubSystem: '<S4>/Publish'
  // Terminate for MATLABSystem: '<S9>/SinkBlock'
  if (!sim_kinematics_DW.obj_c.matlabCodegenIsDeleted) {
    sim_kinematics_DW.obj_c.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S9>/SinkBlock'
  // End of Terminate for SubSystem: '<S4>/Publish'

  // Terminate for Atomic SubSystem: '<S4>/Publish1'
  // Terminate for MATLABSystem: '<S10>/SinkBlock'
  if (!sim_kinematics_DW.obj_f.matlabCodegenIsDeleted) {
    sim_kinematics_DW.obj_f.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S10>/SinkBlock'
  // End of Terminate for SubSystem: '<S4>/Publish1'

  // Terminate for Atomic SubSystem: '<S4>/Publish2'
  // Terminate for MATLABSystem: '<S11>/SinkBlock'
  if (!sim_kinematics_DW.obj_m.matlabCodegenIsDeleted) {
    sim_kinematics_DW.obj_m.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S11>/SinkBlock'
  // End of Terminate for SubSystem: '<S4>/Publish2'

  // Terminate for Atomic SubSystem: '<S4>/Publish3'
  // Terminate for MATLABSystem: '<S12>/SinkBlock'
  if (!sim_kinematics_DW.obj_l.matlabCodegenIsDeleted) {
    sim_kinematics_DW.obj_l.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S12>/SinkBlock'
  // End of Terminate for SubSystem: '<S4>/Publish3'

  // Terminate for Atomic SubSystem: '<S5>/Publish'
  // Terminate for MATLABSystem: '<S14>/SinkBlock'
  if (!sim_kinematics_DW.obj_o.matlabCodegenIsDeleted) {
    sim_kinematics_DW.obj_o.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S14>/SinkBlock'
  // End of Terminate for SubSystem: '<S5>/Publish'

  // Terminate for Atomic SubSystem: '<S5>/Publish1'
  // Terminate for MATLABSystem: '<S15>/SinkBlock'
  if (!sim_kinematics_DW.obj_g.matlabCodegenIsDeleted) {
    sim_kinematics_DW.obj_g.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S15>/SinkBlock'
  // End of Terminate for SubSystem: '<S5>/Publish1'

  // Terminate for Atomic SubSystem: '<S5>/Publish2'
  // Terminate for MATLABSystem: '<S16>/SinkBlock'
  if (!sim_kinematics_DW.obj_p.matlabCodegenIsDeleted) {
    sim_kinematics_DW.obj_p.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S16>/SinkBlock'
  // End of Terminate for SubSystem: '<S5>/Publish2'

  // Terminate for Atomic SubSystem: '<S5>/Publish3'
  // Terminate for MATLABSystem: '<S17>/SinkBlock'
  if (!sim_kinematics_DW.obj.matlabCodegenIsDeleted) {
    sim_kinematics_DW.obj.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S17>/SinkBlock'
  // End of Terminate for SubSystem: '<S5>/Publish3'
  // End of Terminate for SubSystem: '<Root>/motion controller'
}

//
// File trailer for generated code.
//
// [EOF]
//
